#include "NavigationManager.h"
#include "GameManager.h"
#include "Helpers.h"

#include <cmath>
#include <vector>

using namespace std;


vector3df CNavigationManager::getProjectedPosition(CSceneObject *srcObject, f32 time) {
	matrix4 m;
	vector3df res, a, b;

	a = vector3df(0.0f, 0.0f, 0.5f * srcObject->getAcceleration() * std::pow(time,2.0f));
	m.setRotationDegrees(srcObject->getRotation());
	m.transformVect(a);
	
	b = (srcObject->getVelocity() * time);
	res = a + b + srcObject->getPosition();

	return res;
} 

std::vector<vector3df> CNavigationManager::getProjectedCourse(CSceneObject *srcObject, f32 time) {
	matrix4 m;
	f32 timeIter;
	vector3df pos, a, b;
	std::vector<vector3df> res;
	
	for(timeIter=0.5f;timeIter<=time;timeIter+=0.5f) {
		// step forward for our next position at time interval
		a = vector3df(0.0f, 0.0f, 0.5f * srcObject->getAcceleration() * std::pow(timeIter,2.0f));
		m.setRotationDegrees(srcObject->getRotation());
		m.transformVect(a);
		
		// final computation
		b = (srcObject->getVelocity() * timeIter);
		pos = a + b + srcObject->getPosition();

		// final velocity calculations, dont need it
		//a = vector3df(0.0f, 0.0f, srcObject->getAcceleration() * timeIter);
		//m.transformVect(a);
		//vel = a;

		res.push_back(pos);
	}

	return res;
}



bool CNavigationManager::getInterceptCourse(CSceneObject *srcObject, CSceneObject *targetObject, std::vector<vector3df>& interceptCourses, vector<f32> & flightTimes, f32 accel) {
	std::vector<vector3df> velocities, positions;
	
	bool res = this->Intercept(targetObject, srcObject, accel, 0.0f, 0.0f, 600000000, 1, velocities, flightTimes);
	
	if(res) {
		for(unsigned int i = 0; i < velocities.size(); i++) 
			velocities[i] = velocities[i].getHorizontalAngle();

		interceptCourses = velocities;
	}
	return res;
}
/*
This works WITHOUT acceleration
bool CNavigationManager::getInterceptCourse(CSceneObject *srcObject, CSceneObject *targetObject, vector3df & interceptLocation, vector3df & interceptCourse, f32 accel) {
	vector3df t, res, intercept, course;
	
	f32 tempSpeed = 500.0f;

	f32 a = tempSpeed * tempSpeed - targetObject->getVelocity().dotProduct(targetObject->getVelocity());
	f32 b = -2 * targetObject->getVelocity().dotProduct(targetObject->getPosition() - srcObject->getPosition());
	f32 c = -(targetObject->getPosition() - srcObject->getPosition()).dotProduct(targetObject->getPosition() - srcObject->getPosition());
	
	interceptLocation = targetObject->getPosition() + largest_root_of_quadratic_equation(a,b,c) * targetObject->getVelocity();
	
	t = intercept - srcObject->getPosition();
	res = tempSpeed / std::sqrt(t.dotProduct(t))*t;

	interceptCourse = srcObject->getRotationAtTarget(intercept);

	return true;
}
*/
/* Constructors / Destructors */
CNavigationManager::CNavigationManager(void) {
	//SSceneDesc sceneDesc;

	//mSimulator = createPhysxManager(NULL, sceneDesc);
	//mSimulator->setGravity(vector3df(0,0,0));
}

CNavigationManager::CNavigationManager(CSceneObject *obj) {
	CNavigationManager();
	this->setUserObject(obj);
}

CNavigationManager::~CNavigationManager(void) {
	//delete this->mSimulator;
}

bool CNavigationManager::Intercept(CSceneObject *target, CSceneObject *src, f32 projAccel, f32 projVelocity, f32 projPositionOffset, f32 rbt, f32 tolerance, std::vector<vector3df>& velocities, std::vector<f32>& flightTimes) {
	matrix4 m;
	vector3df accel;

	accel = vector3df(0,0,target->getAcceleration());
	m.setRotationDegrees(target->getRotation());
	m.transformVect(accel);

	return (Intercept(target->getPosition(), target->getVelocity(), accel, src->getPosition(), src->getVelocity(), 
						projAccel, projVelocity, projPositionOffset, (int)rbt, tolerance, velocities, flightTimes));
}
bool CNavigationManager::Intercept(vector3df targetPosition, vector3df targetVelocity, vector3df targetAcceleration, 
			   vector3df srcPosition, vector3df srcVelocity, f32 projAccel, f32 projVelocity, f32 projPositionOffset,
			   int rbt, double tolerance, vector<vector3df>& vectors, vector<f32> & flightTimes)
{
     targetVelocity.X -= srcVelocity.X;
     targetVelocity.Y -= srcVelocity.Y;
     targetVelocity.Z -= srcVelocity.Z;
     targetPosition.X -= srcPosition.X;
     targetPosition.Y -= srcPosition.Y;
     targetPosition.Z -= srcPosition.Z;

     if (tolerance < 1.0)
          tolerance = 1.0;
     if (rbt < 0)
          rbt = 0;
     if (rbt == 0)
          projAccel = 0.0;


     double tA = 0.25 * (targetAcceleration.X * targetAcceleration.X + targetAcceleration.Y * targetAcceleration.Y + targetAcceleration.Z * targetAcceleration.Z);
     double tB = targetAcceleration.X * targetVelocity.X + targetAcceleration.Y * targetVelocity.Y + targetAcceleration.Z * targetVelocity.Z;
     double tC = targetVelocity.X * targetVelocity.X + targetAcceleration.X * targetPosition.X + targetVelocity.Y * targetVelocity.Y + targetAcceleration.Y * targetPosition.Y + targetVelocity.Z * targetVelocity.Z + targetAcceleration.Z * targetPosition.Z;
     double tD = 2.0 * (targetVelocity.X * targetPosition.X + targetVelocity.Y * targetPosition.Y + targetVelocity.Z * targetPosition.Z);
     double tE = targetPosition.X * targetPosition.X + targetPosition.Y * targetPosition.Y + targetPosition.Z * targetPosition.Z;

     double fsA = tA - 0.25 * projAccel * projAccel;
     double fsB = tB - projAccel * projVelocity;
     double fsC = tC - projAccel * projPositionOffset - projVelocity * projVelocity;
     double fsD = tD - 2.0 * projVelocity * projPositionOffset;
     double fsE = tE - projPositionOffset * projPositionOffset;

     double ssC = tC - projAccel * projAccel * rbt * rbt - 2.0 * projAccel * projVelocity * rbt - projVelocity * projVelocity;
     double ssD = tD + projAccel * projAccel * rbt * rbt * rbt + projAccel * projVelocity * rbt * rbt - 2.0 * (projAccel * rbt + projVelocity) * projPositionOffset;
     double ssE = tE - 0.25 * projAccel * projAccel * rbt * rbt * rbt * rbt + projAccel * projPositionOffset * rbt * rbt - projPositionOffset * projPositionOffset;
    

     vector<double> roots = vector<double>();
     vector<double> fsroots = rootsOf(fsA, fsB, fsC, fsD, fsE);
     vector<double> fsrootsCA = rootsOf(0.0, 4.0 * fsA, 3.0 * fsB, 2.0 * fsC, fsD);
     vector<double> ssroots = rootsOf(tA, tB, ssC, ssD, ssE);
     vector<double> ssrootsCA = rootsOf(0.0, 4.0 * tA, 3.0 * tB, 2.0 * ssC, ssD);


     for(int i = 0; i != fsroots.size(); ++i)
     {
          double root = fsroots[i];
          if(root >= 0.0 && root <= rbt && does_not_contain(roots, root))
               roots.push_back(root);
     }

     for(int i = 0; i != ssroots.size(); ++i)
     {
          double root = ssroots[i];
          if(root > rbt && does_not_contain(roots, root))
               roots.push_back(root);
     }

     for(int i = 0; i != fsrootsCA.size(); ++i)
     {
          double root = fsrootsCA[i];
          if (tolerance >= abs(fsA * root * root * root * root + fsB * root * root * root + fsC * root * root + fsD * root + fsE) && root >= 0.0 && root <= rbt && does_not_contain(roots, root))
               roots.push_back(root);
     }

     for(int i = 0; i != ssrootsCA.size(); ++i)
     {
          double root = ssrootsCA[i];
          if (tolerance >= abs(tA * root * root * root * root + tB * root * root * root + ssC * root * root + ssD * root + ssE) && root > rbt && does_not_contain(roots, root))
               roots.push_back(root);
     }

     sort(roots.begin(), roots.end());
	

     bool able = roots.size() != 0;

     if(!able)
     {
          vector<double> fsattempts = vector<double>();
          vector<double> ssattempts = vector<double>();

          for(int i = 0; i != fsrootsCA.size(); ++i)
          {
               double root = fsrootsCA[i];
               if (root >= 0.0 && root <= rbt && does_not_contain(fsattempts, root))
                    fsattempts.push_back(root);
          }
          fsattempts.push_back(0);

          for(int i = 0; i != ssrootsCA.size(); ++i)
          {
               double root = ssrootsCA[i];
               if(root > rbt && does_not_contain(ssattempts, root))
                    ssattempts.push_back(root);
          }

          vector<double> keys = vector<double>(fsattempts.size() + ssattempts.size());
          vector<double> items = vector<double>(fsattempts.size() + ssattempts.size());

          for (int i = 0; i != fsattempts.size(); ++i)
          {
               keys[i] = abs(fsA * fsattempts[i] * fsattempts[i] * fsattempts[i] * fsattempts[i] + fsB * fsattempts[i] * fsattempts[i] * fsattempts[i] + fsC * fsattempts[i] * fsattempts[i] + tD * fsattempts[i] + tE);
               items[i] = fsattempts[i];
          }

          for (int i = 0; i != ssattempts.size(); ++i)
          {
               keys[fsattempts.size() + i] = abs(tA * ssattempts[i] * ssattempts[i] * ssattempts[i] * ssattempts[i] + tB * ssattempts[i] * ssattempts[i] * ssattempts[i] + ssC * ssattempts[i] * ssattempts[i] + ssD * ssattempts[i] + ssE);
               items[fsattempts.size() + i] = ssattempts[i];
          }

          keysort(keys, items);

		  for(int i = 0; i != items.size(); ++i) {
			  roots.push_back(items[i]);
		  }
     }


     vectors = vector<vector3df>(roots.size());
	 flightTimes = vector<f32>(roots.size());
     for(int i = 0; i != roots.size(); ++i)
     {
          vector3df v;
         
          if(projAccel == 0.0 && projVelocity == 0.0)
          {
               v.X = srcVelocity.X;
               v.Y = srcVelocity.Y;
               v.Z = srcVelocity.Z;
          }
          else
          {
			  //(Pt = (1/2)At2 + V0t + P0)
               v.X = 0.5 * targetAcceleration.X * roots[i] * roots[i] + targetVelocity.X * roots[i] + targetPosition.X;
               v.Y = 0.5 * targetAcceleration.Y * roots[i] * roots[i] + targetVelocity.Y * roots[i] + targetPosition.Y;
               v.Z = 0.5 * targetAcceleration.Z * roots[i] * roots[i] + targetVelocity.Z * roots[i] + targetPosition.Z;

          }

          double m = sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
          if(m < EPSILON)
               m = 1.0;

		  flightTimes[i] = roots[i];

          vectors[i].X = v.X / m;
          vectors[i].Y = v.Y / m;
          vectors[i].Z = v.Z / m;
     }


     return able;
}
